/*
 * Copyright (c) 2017, AEye
 * All rights reserved.
 */

#include <algorithm>
#include <cstring>
#include <cmath>
#include <iostream>
#include <memory>
#include <sstream>

#include "SensorCommander.hpp"
#include "AEyeExceptions.hpp"
#include "ecast.h"
#include "AEyeSensorAPI.h"
#include "AEyeSDKEnumsToStr.hpp"
#include "spdlog/spdlog.h"
#include "spdlog/fmt/bundled/ostream.h"

#ifdef _MSC_VER
#define _WINSOCKAPI_
#include <Windows.h>
#include <winsock2.h>
#include <stdint.h>
#include <tchar.h>
#include <dbghelp.h>
#include "timetools.h"
#define CHECK_BODY_TYPE_AND_SIZE(retHeader, packetType, packetBody) checkBodyTypeAndSize(__FUNCSIG__, retHeader, packetType, packetBody)
#define CHECK_RET_PACKET_STATUS(retStatus) checkRetPacketStatus(__FUNCSIG__, retStatus)

#include <Ws2tcpip.h>
bool isValidIpAddress(const std::string& ip_address) {
  SOCKADDR_STORAGE addr = {0};
  CHAR szAddr[NI_MAXHOST] = {0};
  PVOID pAddr = NULL;
  if (!InetPton((INT)addr.ss_family, szAddr, pAddr)) {
    spdlog::error(ip_address + " is an invalid IP.");
    return false;
  }
  return true;
}
#else
#include <sys/time.h> // NOLINT (different header)
#include <cxxabi.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <sys/socket.h>
#define CHECK_BODY_TYPE_AND_SIZE(retHeader, packetType, packetBody) checkBodyTypeAndSize(__PRETTY_FUNCTION__, retHeader, packetType, packetBody)
#define CHECK_RET_PACKET_STATUS(retStatus) checkRetPacketStatus(__PRETTY_FUNCTION__, retStatus)

template<typename T>
std::string typeName() {
  int status;
  std::string tName = typeid(T).name();
  char* demangledName = abi::__cxa_demangle(tName.c_str(), nullptr, nullptr, &status);
  if (status == 0) {
    tName = demangledName;
    std::free(demangledName);
  }
  return tName;
}

bool isValidIpAddress(const std::string& ip_address) {
  struct sockaddr_in unused{};
  int status = inet_pton(AF_INET, ip_address.c_str(), &unused.sin_addr);
  if (status == 0) {
      spdlog::error(ip_address + " is an invalid IP.");
      return false;
  }
  else if (status == -1) {
      spdlog::error("AF_INET is not a valid IP address family.");
      return false;
  }
  return true;
}
#endif

using namespace aeye;
using aeye::SensorCommander;

namespace {
void checkRetPacketStatus(const std::string &funcName, int16_t status) {
  ReturnStatus reStatus = toEnum<ReturnStatus>(status);
  if (reStatus == ReturnStatus::NO_ERRORS) return;

  auto ss = fmt::format("Error in {}. Packet return status: {}.", funcName, ToStr(reStatus));
  spdlog::error(ss);
  throw return_status_error(ss, reStatus);
}
template<typename T>
std::string get_type_name() {
#ifdef _MSC_VER
  return typeid(T).name();
#else
  return typeName<T>();
#endif
}
}

#define S_TIMEOUT 100000 // 1000 mSec = 1 Sec
#define S_TIMEOUT_100 100 // 100 mSec

void SensorCommander::checkBodyType(const std::string &functionName,
                                    const PacketHeader &returnHeader,
                                    AEyeCommands packetType) {
  if (returnHeader.type == ut_cast(packetType)) return;

  auto ss = fmt::format("Incorrect body type in {}. Returned packet type: {}.", functionName, returnHeader.type);
  spdlog::error(ss);
  try {
    socket_.cleanUp(S_TIMEOUT_100);
  } catch (std::exception& e) {}
  throw aeye::sensor_comm_error(ss);
}

void SensorCommander::checkBodySize(const std::string& functionName, const std::string& typeName, const PacketHeader& returnHeader,
                                    size_t size) {
  if (returnHeader.bodySize == size) return;

  std::string ss = fmt::format("Incorrect BodySize in {}. Actual: {} != sizeof({}): {}.", functionName,
                               returnHeader.bodySize, typeName, size);
  spdlog::error(ss);

  try {
    socket_.cleanUp(S_TIMEOUT_100);
  } catch (std::exception& e) {}
  throw aeye::sensor_comm_error(ss);
}

template <typename T>
void SensorCommander::checkBodyTypeAndSize(const std::string& functionName, const PacketHeader& returnHeader,
                                           aeye::AEyeCommands packetType, const T&) {
  checkBodyType(functionName, returnHeader, packetType);
  checkBodySize(functionName, get_type_name<T>(), returnHeader, sizeof(T));
}

void SensorCommander::ReceivePacketBody(void *body, size_t size) {
  size_t length = socket_.receive(body, size, S_TIMEOUT);
  if (length == size) return;

  std::string err = fmt::format("Received packet body size {} != expected size {}.", length, size);
  spdlog::error(err);
  throw aeye::sensor_comm_error(err);
}


void SensorCommander::sendCommand(const aeye::PacketHeader& header, const void* data, size_t size)
{
    size_t length = socket_.send(&header, sizeof(header), S_TIMEOUT);
    if (length != sizeof(header)) {
      std::stringstream ss;
#ifdef _MSC_VER
      ss << "Error in " << __FUNCSIG__ << ". \n(sentBytes:" << length << " != sizeof(header):" << sizeof(header) << ")" << std::endl;
#else
      ss << "Error in " << __PRETTY_FUNCTION__ << ". \n(sentBytes:" << length << " != sizeof(header):" << sizeof(header) << ")" << std::endl;
#endif
      std::string err = ss.str();
      spdlog::error(err);
      throw aeye::sensor_comm_error(err);
    }

    if (data) {
      length = socket_.send(data, size, S_TIMEOUT);
      if (length != size) {
        std::stringstream ss;
#ifdef _MSC_VER
        ss << "Error in " << __FUNCSIG__ << ". \n(sentBytes:" << length << " != dataSize:" << size << ")" << std::endl;
#else
        ss << "Error in " << __PRETTY_FUNCTION__ << ". \n(sentBytes:" << length << " != dataSize:" << size << ")" << std::endl;
#endif
        std::string err = ss.str();
        spdlog::error(err);
        throw aeye::sensor_comm_error(err);
      }
    }
}

PacketReturnDefault SensorCommander::sendCommandReceiveDefault(const aeye::PacketHeader& header, const void* data, size_t size)
{
    sendCommand(header, data, size);
    PacketReturnDefault packetReturnDefault = readDefaultResponse();
    return packetReturnDefault;
}

void SensorCommander::sendCommand(aeye::AEyeCommands packet_type) {
    aeye::PacketHeader header = fillHeader(packet_type);
    header.bodySize = 0;
    sendCommand(header, nullptr, 0);
}

aeye::PacketHeader SensorCommander::fillHeader(aeye::AEyeCommands packetType) {
  aeye::PacketHeader header;
  // memset(&header, 0, sizeof(aeye::PacketHeader));
  header.type = ut_cast(packetType);
  auto it = commandVersionsMap.find(packetType);
  if (it != commandVersionsMap.end()) {
    header.version = it->second;
  }
  else {
    throw aeye::sensor_comm_error("Requested unknown packet!");
  }
  //initialize the rest
  header.flags = 0;
  header.tsSec = 0;
  header.tsuSec = 0;
  return header;
}

PacketReturnDefault SensorCommander::sendCommandReceiveDefault(aeye::AEyeCommands packetType) {
    aeye::PacketHeader header = fillHeader(packetType);
    header.bodySize = 0;
    return sendCommandReceiveDefault(header, nullptr, 0);
}

template <typename T>
void SensorCommander::sendCommand(aeye::AEyeCommands packetType, const T* packet) {
    aeye::PacketHeader header = fillHeader(packetType);
    header.bodySize = sizeof(T);
    sendCommand(header, packet, sizeof(T));
}

template <typename T>
PacketReturnDefault SensorCommander::sendCommandReceiveDefault(aeye::AEyeCommands packetType, const T* packet) {
    aeye::PacketHeader header = fillHeader(packetType);
    header.bodySize = sizeof(T);
    return sendCommandReceiveDefault(header, packet, sizeof(T));
}

aeye::PacketHeader SensorCommander::readResponseHeader()
{
    // Receive response
    aeye::PacketHeader returnHeader;

    size_t length = socket_.receive(&returnHeader, sizeof(returnHeader), S_TIMEOUT);
    if (length != sizeof(returnHeader)) {
        throw aeye::sensor_comm_error();
    }

    // Validation header
    static aeye::PacketHeader valid;

    // Check marker
    if (returnHeader.marker[0] != valid.marker[0] ||
        returnHeader.marker[1] != valid.marker[1] ||
        returnHeader.marker[2] != valid.marker[2] ||
        returnHeader.marker[3] != valid.marker[3]) {
        // Not an AEye packet
        throw aeye::sensor_comm_error("Sensor communication error. Wrong marker in returned header");
    }
    return returnHeader;
}

PacketReturnDefault SensorCommander::readDefaultResponse()
{
  // Receive response
  aeye::PacketHeader returnHeader = readResponseHeader();
  PacketReturnDefault packetReturnDefault;

  CHECK_BODY_TYPE_AND_SIZE(returnHeader, aeye::RET_DEFAULT, packetReturnDefault);
  ReceivePacketBody(&packetReturnDefault, sizeof(packetReturnDefault));

  return packetReturnDefault;
}

PacketBuildInfoBody SensorCommander::readBuildInfoResponse(std::string &infoStr)
{
    // Receive response
    aeye::PacketHeader returnHeader = readResponseHeader();
    PacketBuildInfoBody packetBuildInfoBody;

#ifdef _MSC_VER
    checkBodyType(__FUNCSIG__, returnHeader, aeye::RET_BUILD_INFO);
#else
    checkBodyType(__PRETTY_FUNCTION__, returnHeader, aeye::RET_BUILD_INFO);
#endif

    if (returnHeader.bodySize < sizeof(PacketBuildInfoBody)) {
      std::stringstream ss;
#ifdef _MSC_VER
      ss << "Error in " << __FUNCSIG__ << ". BodySize:" << returnHeader.bodySize
#else
      ss << "Error in " << __PRETTY_FUNCTION__ << ". BodySize:" << returnHeader.bodySize
#endif
         << " < sizeof(PacketBuildInfoBody)):" << sizeof(PacketBuildInfoBody) << std::endl;
      std::string err = ss.str();
      spdlog::error(err);
      try {
        size_t bodySize = sizeof(PacketBuildInfoBody);
        std::unique_ptr<char[]> tempBuff(new char[bodySize]);
        socket_.receive(tempBuff.get(), bodySize, S_TIMEOUT_100);
      } catch (std::exception& e) {}
      throw aeye::sensor_comm_error(err);
    }

  ReceivePacketBody(&packetBuildInfoBody, sizeof(packetBuildInfoBody));

    size_t infoSize = (returnHeader.bodySize - sizeof(packetBuildInfoBody)) / sizeof(char);

    std::unique_ptr<char[]> info(new char[infoSize]);
    size_t length = socket_.receive(info.get(), infoSize, S_TIMEOUT);
    if (length != infoSize) {
      std::string err = fmt::format("Received size {} != infoSize {}.", length, infoSize);
      spdlog::error(err);
      throw aeye::sensor_comm_error(err);
    }

    info[infoSize - 1] = 0;
    infoStr = std::string(info.get());

    return packetBuildInfoBody;
}

PacketHWModelInfoBody SensorCommander::readGetHWModelTypeResponse() {
  // Receive response
  aeye::PacketHeader returnHeader = readResponseHeader();
  PacketHWModelInfoBody packetHWModelTypeInfoBody;

  CHECK_BODY_TYPE_AND_SIZE(returnHeader, aeye::RET_HW_MODEL, packetHWModelTypeInfoBody);
  ReceivePacketBody(&packetHWModelTypeInfoBody, sizeof(packetHWModelTypeInfoBody));

  return packetHWModelTypeInfoBody;
}

PacketGetPoseBody SensorCommander::readPoseResponse() {
  // Receive response
  PacketHeader returnHeader = readResponseHeader();
  PacketGetPoseBody packetPoseBodyReturn;

  CHECK_BODY_TYPE_AND_SIZE(returnHeader, aeye::RET_POSE_VALUE, packetPoseBodyReturn);
  ReceivePacketBody(&packetPoseBodyReturn, sizeof(packetPoseBodyReturn));

  return packetPoseBodyReturn;
}


PacketGetPassiveSensingStateBody SensorCommander::readPassiveSensingStateResponse() {
  // Receive response
  PacketHeader returnHeader = readResponseHeader();
  PacketGetPassiveSensingStateBody packetPassiveSensingStateBody;

  CHECK_BODY_TYPE_AND_SIZE(returnHeader, aeye::RET_PASSIVE_SENSING_STATE, packetPassiveSensingStateBody);
  ReceivePacketBody(&packetPassiveSensingStateBody, sizeof(packetPassiveSensingStateBody));

  return packetPassiveSensingStateBody;
}

PacketGetERPsStateBody SensorCommander::readERPsStateResponse() {
  // Receive response
  PacketHeader returnHeader = readResponseHeader();
  PacketGetERPsStateBody packetERPsStateBody;

  CHECK_BODY_TYPE_AND_SIZE(returnHeader, aeye::RET_ERPS_SENSING_STATE, packetERPsStateBody);
  ReceivePacketBody(&packetERPsStateBody, sizeof(packetERPsStateBody));

  return packetERPsStateBody;
}

PacketGetLaserStateBodyReturnV2 SensorCommander::readLaserStateResponse() {
  // Receive response
  PacketHeader returnHeader = readResponseHeader();
  PacketGetLaserStateBodyReturnV2 packetLaserStateBodyV2;

  if (returnHeader.version == RET_LASER_STATE_VERSION) {
    CHECK_BODY_TYPE_AND_SIZE(returnHeader, aeye::RET_LASER_STATE, packetLaserStateBodyV2);
    ReceivePacketBody(&packetLaserStateBodyV2, sizeof(packetLaserStateBodyV2));
  }
  else {
    throw aeye::sensor_comm_error("Unsupported protocol version for aeye::RET_LASER_STATE");
  }
  CHECK_RET_PACKET_STATUS(packetLaserStateBodyV2.status);
  return packetLaserStateBodyV2;
}

PacketGetRangingStateBodyReturn SensorCommander::readRangingStateResponse() {
  // Receive response
  PacketHeader returnHeader = readResponseHeader();
  PacketGetRangingStateBodyReturn packetRangingStateBody;

  CHECK_BODY_TYPE_AND_SIZE(returnHeader, aeye::RET_RANGING_STATE, packetRangingStateBody);
  ReceivePacketBody(&packetRangingStateBody, sizeof(packetRangingStateBody));

  return packetRangingStateBody;
}

SensorCommander::SensorCommander(const std::string& addr, uint16_t cmd_port) : addr_(addr), cmd_port_(cmd_port),
                                 commandVersionsMap(BuildCommandVersionMap()) {
}

void SensorCommander::disconnectFromSensor() {
    if (socket_.isOpen()){
      socket_.close();
    }
}

bool SensorCommander::isConnected() {
  return socket_.isOpen();
}

void SensorCommander::connectToSensor() {
  // Connect to sensor
  if (!socket_.isOpen()) {
    if (!socket_.connect(addr_, cmd_port_, 1)) {
      throw aeye::sensor_comm_error(-1);
    }
    else { // sync clock
      setTime();
    }
  }
}

void SensorCommander::cmdStart(const std::string& client_addr, uint16_t range_port) {
  PacketStartBody packet;

  memset(packet.clientIp, 0, sizeof(packet.clientIp));
  memcpy(packet.clientIp, client_addr.c_str(), std::min(client_addr.size(), sizeof(packet.clientIp)));
  packet.rangePort  = range_port;

  PacketReturnDefault packetReturnDefault = sendCommandReceiveDefault(aeye::CMD_START, &packet);
  CHECK_RET_PACKET_STATUS(packetReturnDefault.status);
}

void SensorCommander::cmdStop() {
  PacketReturnDefault packetReturnDefault = sendCommandReceiveDefault(aeye::CMD_STOP);
  CHECK_RET_PACKET_STATUS(packetReturnDefault.status);
}

void SensorCommander::cmdReboot() {
  PacketReturnDefault packetReturnDefault = sendCommandReceiveDefault(aeye::CMD_REBOOT);
  CHECK_RET_PACKET_STATUS(packetReturnDefault.status);
}

void SensorCommander::setPassiveSensingMode(uint8_t passiveModeMask) {
  PacketSetPassiveModeBody body;
  body.mode = passiveModeMask;

  PacketReturnDefault returnedPacket = sendCommandReceiveDefault(aeye::CMD_SET_PASSIVE_SENSING_MODE, &body);
  CHECK_RET_PACKET_STATUS(returnedPacket.status);
}

void SensorCommander::setPassiveModeParameter(PassiveModeParam parameter, float value)  {
  PacketSetPassiveParamBody body;
  body.param = parameter;
  body.value = value;

  PacketReturnDefault returnedPacket = sendCommandReceiveDefault(aeye::CMD_SET_PASSIVE_MODE_PARAMETER, &body);
  CHECK_RET_PACKET_STATUS(returnedPacket.status);
}

void SensorCommander::setNtpServer(bool en, const std::string& ip) {
    if (ip.length() > (sizeof(aeye::PacketSetNtpBody::serverIp) - 1)) {
      throw std::runtime_error("NTP server IP address exceeds acceptable length.");
    }
    PacketSetNtpBody body{};
    std::memcpy(body.serverIp, ip.c_str(), sizeof(body.serverIp));
    body.useNTP = static_cast<uint8_t>(en);

    try {
      PacketReturnDefault packetReturnDefault = sendCommandReceiveDefault(aeye::CMD_SET_NTP_SERVER, &body);
      CHECK_RET_PACKET_STATUS(packetReturnDefault.status);
    } catch (std::exception &e) {
      spdlog::error("SensorCommander::setNtpServer() : send_command failed.");
    }
}

PacketGetNtpStatus SensorCommander::getNtpStatus() {
    sendCommand(aeye::CMD_GET_NTP_STATUS);
    PacketHeader returnHeader = readResponseHeader();
    PacketGetNtpStatus body;
    CHECK_BODY_TYPE_AND_SIZE(returnHeader, aeye::CMD_GET_NTP_STATUS, body);
  ReceivePacketBody(&body, sizeof(body));
    CHECK_RET_PACKET_STATUS(body.status);
    return body;
}

void SensorCommander::setPtpServer(bool en) {
    PacketSetPtpBody body;
    body.usePTP = static_cast<uint8_t>(en);
    try {
        PacketReturnDefault packetReturnDefault = sendCommandReceiveDefault(aeye::CMD_SET_PTP_SERVER, &body);
        CHECK_RET_PACKET_STATUS(packetReturnDefault.status);
    } catch (std::exception &e) {
      throw std::runtime_error("SensorCommander::setPtpServer() : send_command failed.");
    }
}

PacketGetPtpStatus SensorCommander::getPtpStatus() {
    sendCommand(aeye::CMD_GET_PTP_STATUS);
    PacketHeader returnHeader = readResponseHeader();
    PacketGetPtpStatus body;
    CHECK_BODY_TYPE_AND_SIZE(returnHeader, aeye::CMD_GET_PTP_STATUS, body);
  ReceivePacketBody(&body, sizeof(body));
    CHECK_RET_PACKET_STATUS(body.status);
    return body;
}

void SensorCommander::setGptpServer(bool en) {
  PacketSetGptpBody body{static_cast<uint8_t>(en)};
  try {
    PacketReturnDefault packetReturnDefault = sendCommandReceiveDefault(aeye::CMD_SET_GPTP_SERVER, &body);
    CHECK_RET_PACKET_STATUS(packetReturnDefault.status);
  } catch (std::exception &e) {
    throw std::runtime_error("SensorCommander::setGptpServer() : send_command failed.");
  }
}

PacketGetGptpStatus SensorCommander::getGptpStatus() {
  sendCommand(aeye::CMD_GET_GPTP_STATUS);
  PacketHeader returnHeader = readResponseHeader();
  PacketGetGptpStatus body;
  CHECK_BODY_TYPE_AND_SIZE(returnHeader, aeye::CMD_GET_GPTP_STATUS, body);
  ReceivePacketBody(&body, sizeof(body));
  CHECK_RET_PACKET_STATUS(body.status);
  return body;
}

void SensorCommander::setNetworkConfig(const std::string& server_ip, const std::string& netmask,
                                     const std::string& gateway, uint16_t cmdport) {
  if (server_ip.length() > (sizeof(PacketNetworkConfigBody::serverIp) - 1)) {
    throw std::runtime_error("Server IP address exceeds acceptable length.");
  }
  PacketNetworkConfigBody body{};

  // Note: interface is not passed down from here
  std::memcpy(body.serverIp, server_ip.c_str(), server_ip.length());
  std::memcpy(body.netmask, netmask.c_str(), netmask.length());
  std::memcpy(body.gateway, gateway.c_str(), gateway.length());
  body.commandPort = cmdport;

  try {
    PacketReturnDefault packetReturnDefault = sendCommandReceiveDefault(aeye::CMD_SET_NETWORK_CONFIG, &body);
    CHECK_RET_PACKET_STATUS(packetReturnDefault.status);
  } catch (std::exception &e) {
    spdlog::error("SensorCommander::sendNetworkConfig() : send_command raised exception `{}`.", e.what());
    throw;
  }
}

void SensorCommander::getNetworkConfig(std::string& server_ip, std::string& netmask,
                                       std::string& gateway, uint16_t& cmdport) {

  sendCommand(aeye::CMD_GET_NETWORK_CONFIG);
  PacketHeader returnHeader = readResponseHeader();
  PacketNetworkConfigBody body;
  CHECK_BODY_TYPE_AND_SIZE(returnHeader, aeye::CMD_GET_NETWORK_CONFIG, body);
  ReceivePacketBody(&body, sizeof(body));

  std::string server_ip_tmp = body.serverIp;
  std::string netmask_tmp = body.netmask;
  std::string gateway_tmp = body.gateway;
  uint16_t cmdport_tmp = body.commandPort;

  // Check the incoming data
  if (server_ip_tmp.empty())
    throw aeye::return_status_error("Empty sensor IP address received.", ReturnStatus::BAD_PARAMETER);

  if (!isValidIpAddress(server_ip_tmp))
    throw aeye::return_status_error("Invalid sensor IP address received.", ReturnStatus::BAD_PARAMETER);

  if (netmask_tmp.empty())
    throw aeye::return_status_error("Empty sensor netmask received.", ReturnStatus::BAD_PARAMETER);

  if (!isValidIpAddress(netmask_tmp))
    throw aeye::return_status_error("Invalid netmask address received.", ReturnStatus::BAD_PARAMETER);

  if (gateway_tmp.empty())
    throw aeye::return_status_error("Empty sensor gateway IP address received.", ReturnStatus::BAD_PARAMETER);

  if (!isValidIpAddress(gateway_tmp))
    throw aeye::return_status_error("Invalid gateway IP address received.", ReturnStatus::BAD_PARAMETER);

  // If all checks pass, assign the values to the args passed by reference
  server_ip = server_ip_tmp;
  netmask = netmask_tmp;
  gateway = gateway_tmp;
  cmdport = cmdport_tmp;
}

void SensorCommander::setTime() {
  struct timeval  tv{};
#ifdef _MSC_VER
  struct timetools::timezone tz;
  if (timetools::gettimeofday(&tv, &tz) == -1) {
#else
  struct timezone tz{};
  if (gettimeofday(&tv, &tz) == -1) {
#endif
    spdlog::error("SensorCommander::setTime() : gettimeofday failed");
    return;
  }
  else {
    PacketTimeBody body;

    body.tvSec         = static_cast<uint32_t>(tv.tv_sec);
    body.tvuSec        = static_cast<uint32_t>(tv.tv_usec);
    body.tzMinutesWest = tz.tz_minuteswest;
    body.tzDstTime     = tz.tz_dsttime;

    try {
      PacketReturnDefault packetReturnDefault = sendCommandReceiveDefault(aeye::CMD_SET_TIME, &body);
      CHECK_RET_PACKET_STATUS(packetReturnDefault.status);
    } catch (std::exception& e) {
      spdlog::warn("SensorCommander::setTime() : send_command : failed");
    }
  }
}

std::string SensorCommander::getShotlistName(uint16_t i) {

  std::string retStr;
  try {

    // Prepare request packet
    PacketGetShotlistNameBody body;
    body.shotlistIdx = i;

    // Send the request packet
    sendCommand(aeye::CMD_GET_SHOTLIST_NAME, &body);

    // Receive response
    PacketReturnShotlistNameBody packetReturnShotlistNameBody = readGetShotListNameResponse(retStr);

    // Check the ReturnStatus
    if (packetReturnShotlistNameBody.status == ut_cast(ReturnStatus::BAD_PARAMETER)) {
      return "";
    }
    CHECK_RET_PACKET_STATUS(packetReturnShotlistNameBody.status);
  } catch (std::exception& e) {
    spdlog::error("getShotlistName raised exception: `{}`.", e.what());
    throw;
  }

  return retStr;

}

PacketReturnShotlistNameBody SensorCommander::readGetShotListNameResponse(std::string& infoStr) {
  // Receive response
  aeye::PacketHeader returnHeader = readResponseHeader();
  PacketReturnShotlistNameBody packetReturnShotlistNameBody;

#ifdef _MSC_VER
  checkBodyType(__FUNCSIG__, returnHeader, aeye::RET_SHOTLIST_NAME);
#else
  checkBodyType(__PRETTY_FUNCTION__, returnHeader, aeye::RET_SHOTLIST_NAME);
#endif

  if (returnHeader.bodySize < sizeof(PacketReturnShotlistNameBody)) {
    std::stringstream ss;
#if defined(_WIN32) || defined(WIN64)
    ss << "Error in " << __FUNCSIG__ << ". BodySize:" << returnHeader.bodySize
#else
    ss << "Error in " << __PRETTY_FUNCTION__ << ". BodySize:" << returnHeader.bodySize
#endif
       << " < sizeof(PacketBuildInfoBody)):" << sizeof(PacketBuildInfoBody) << std::endl;
    std::string err = ss.str();
    spdlog::error(err);
    try {
      size_t bodySize = sizeof(PacketReturnShotlistNameBody);
      std::unique_ptr<char[]> tempBuff(new char[bodySize]);
      socket_.receive(tempBuff.get(), bodySize, S_TIMEOUT_100);
    } catch (std::exception& e) {}
    throw aeye::sensor_comm_error(err);
  }

  ReceivePacketBody(&packetReturnShotlistNameBody, sizeof(packetReturnShotlistNameBody));

  // Get the number of characters in the string
  uint8_t infoSize = packetReturnShotlistNameBody.stringLength;

  std::unique_ptr<char[]> info(new char[infoSize]);
  size_t length = socket_.receive(info.get(), infoSize, S_TIMEOUT);
  if (length != infoSize) {
    std::string err = fmt::format("Received size: {} != infoSize: {}.", length, infoSize);
    spdlog::error(err);
    throw aeye::sensor_comm_error(err);
  }

  info[infoSize - 1] = 0;
  infoStr = std::string(info.get());

  return packetReturnShotlistNameBody;
}

void SensorCommander::setSendMarkerPoints(bool value) {
  PacketSendMarkerPoints body;
  body.sendMarkerPoints = (value)? 1 : 0;

  PacketReturnDefault packetReturnDefault = sendCommandReceiveDefault(aeye::CMD_RET_MARKER_POINTS, &body);
  CHECK_RET_PACKET_STATUS(packetReturnDefault.status);
}

void SensorCommander::writeScannerRegister(uint32_t addr, uint32_t value) {
  PacketSetRegister32Body body;
  body.address = addr;
  body.value   = value;
  PacketReturnDefault packetReturnDefault;
  packetReturnDefault = sendCommandReceiveDefault(aeye::CMD_WRITE_SCANNER_REGISTER, &body);
  CHECK_RET_PACKET_STATUS(packetReturnDefault.status);
}

uint32_t SensorCommander::readScannerRegister(uint32_t addr) {
  aeye::PacketGetRegister32Body body;
  body.address = addr;
  sendCommand(aeye::CMD_READ_SCANNER_REGISTER, &body);

  // Receive response
  aeye::PacketHeader      returnHeader = readResponseHeader();
  PacketRegister32Body packetRegisterBodyReturn;

  CHECK_BODY_TYPE_AND_SIZE(returnHeader, aeye::RET_SCANNER_REGISTER_VALUE, packetRegisterBodyReturn);
  ReceivePacketBody(&packetRegisterBodyReturn, sizeof(packetRegisterBodyReturn));

  CHECK_RET_PACKET_STATUS(packetRegisterBodyReturn.status);

  auto regValue = packetRegisterBodyReturn.registerValue;
  return regValue;
}

void SensorCommander::writeReceiverRegister(uint32_t addr, uint32_t value) {
  PacketSetRegister32Body body;
  body.address = addr;
  body.value   = value;
  PacketReturnDefault packetReturnDefault;
  packetReturnDefault = sendCommandReceiveDefault(aeye::CMD_SET_RECEIVER_REGISTER, &body);
  CHECK_RET_PACKET_STATUS(packetReturnDefault.status);
}

uint32_t SensorCommander::readReceiverRegister(uint32_t addr) {
  aeye::PacketGetRegister32Body body;
  body.address = addr;
  sendCommand(aeye::CMD_GET_RECEIVER_REGISTER, &body);

  // Receive response
  aeye::PacketHeader      returnHeader = readResponseHeader();
  PacketRegister32Body packetRegisterBodyReturn;

  CHECK_BODY_TYPE_AND_SIZE(returnHeader, aeye::RET_RECEIVER_REGISTER_VALUE, packetRegisterBodyReturn);
  ReceivePacketBody(&packetRegisterBodyReturn, sizeof(packetRegisterBodyReturn));

  CHECK_RET_PACKET_STATUS(packetRegisterBodyReturn.status);

  auto regValue = packetRegisterBodyReturn.registerValue;
  return regValue;
}

std::pair<std::string, PacketBuildInfoBody> SensorCommander::cmdGetBuildInfo() {
  std::string retStr;
  try {
    sendCommand(aeye::CMD_GET_BUILD_INFO);
    PacketBuildInfoBody buildInfoBody = readBuildInfoResponse(retStr);
    CHECK_RET_PACKET_STATUS(buildInfoBody.status);
    return {retStr, buildInfoBody};
  } catch (std::exception& e) {
    spdlog::error("cmdGetBuildInfo raised exception: `{}`.", e.what());
    throw;
  }
}

ModelType SensorCommander::cmdGetSensorModel() {
  ModelType retModelType = ModelType::UNKNOWN;
  try {
    sendCommand(aeye::CMD_GET_HW_MODEL);
    PacketHWModelInfoBody HWModelInfoBody = readGetHWModelTypeResponse();
    retModelType = HWModelInfoBody.hwModel;
  } catch (std::exception& e) {
    spdlog::error("cmdGetSensorModel raised exception: `{}`.", e.what());
    throw;
  }
  return retModelType;
}

PacketGetPoseBody SensorCommander::getPoseFromSensor() {
  sendCommand(aeye::CMD_GET_POSE);
  PacketGetPoseBody getPoseBody = readPoseResponse();
  CHECK_RET_PACKET_STATUS(getPoseBody.status);
  return getPoseBody;
}

PacketGetPassiveSensingStateBody SensorCommander::getPassiveSensingState() {
  sendCommand(aeye::CMD_GET_PASSIVE_SENSING_STATE);
  PacketGetPassiveSensingStateBody passiveSensingStateBody = readPassiveSensingStateResponse();
  CHECK_RET_PACKET_STATUS(passiveSensingStateBody.status);
  return passiveSensingStateBody;
}

PacketGetLaserStateBodyReturnV2 SensorCommander::getLaserState() {
  sendCommand(aeye::CMD_GET_LASER_STATE);
  PacketGetLaserStateBodyReturnV2 laserStateBody = readLaserStateResponse();
  CHECK_RET_PACKET_STATUS(laserStateBody.status);
  return laserStateBody;
}

PacketGetRangingStateBodyReturn SensorCommander::getRangingState() {
  sendCommand(aeye::CMD_GET_RANGING_STATE);
  PacketGetRangingStateBodyReturn rangeDelayStateBody = readRangingStateResponse();
  CHECK_RET_PACKET_STATUS(rangeDelayStateBody.status);
  return rangeDelayStateBody;
}

void SensorCommander::setLaserEnable(uint8_t enable) {
  PacketSetLaserEnable body;
  body.enable = enable;
  PacketReturnDefault packetReturnDefault = sendCommandReceiveDefault(aeye::CMD_SET_LASER_ENABLE, &body);
  CHECK_RET_PACKET_STATUS(packetReturnDefault.status);
}

void SensorCommander::setLaserPower(float value) {
  PacketLaserConfigFloatBody body{value};
  PacketReturnDefault packetReturnDefault = sendCommandReceiveDefault(aeye::CMD_SET_LASER_CONFIG_FLOAT, &body);
  CHECK_RET_PACKET_STATUS(packetReturnDefault.status);
}

void SensorCommander::setRangeDelay(float value) {
  PacketSetRangeGateDelayBody body;
  body.rangeGateDelay = value;
  PacketReturnDefault packetReturnDefault = sendCommandReceiveDefault(aeye::CMD_SET_RANGE_GATE_DELAY, &body);
  CHECK_RET_PACKET_STATUS(packetReturnDefault.status);
}

void SensorCommander::setPose(double x, double y, double z, double yaw, double pitch, double roll) {
  PacketSetPoseBody body;
  body.x = x;
  body.y = y;
  body.z = z;
  body.yaw = yaw;
  body.pitch = pitch;
  body.roll = roll;
  PacketReturnDefault packetReturnDefault = sendCommandReceiveDefault(aeye::CMD_SET_POSE, &body);
  CHECK_RET_PACKET_STATUS(packetReturnDefault.status);
}

void SensorCommander::setPointReturnMask(uint16_t pointReturnMask) {
  PacketSetPointReturnMask packetSetPointReturnMask;
  packetSetPointReturnMask.pointReturnMask = pointReturnMask;
  PacketReturnDefault packetReturnDefault = sendCommandReceiveDefault(aeye::CMD_SET_POINT_RETURN_MASK, &packetSetPointReturnMask);
  CHECK_RET_PACKET_STATUS(packetReturnDefault.status);
}

void SensorCommander::setIntensityType(uint8_t intensityType) {
  PacketSetIntensityType packetSetIntensityType;
  packetSetIntensityType.value = intensityType;
  PacketReturnDefault packetReturnDefault = sendCommandReceiveDefault(aeye::CMD_SET_INTENSITY_TYPE, &packetSetIntensityType);
  CHECK_RET_PACKET_STATUS(packetReturnDefault.status);
}

void SensorCommander::setSkirtThreshNearHigh(uint16_t value) {
  PacketSetSkirtThreshValue packet;
  packet.value = value;
  PacketReturnDefault returnDefault = sendCommandReceiveDefault(aeye::CMD_SET_SKIRT_THRESH_NEAR_HIGH, &packet);
  CHECK_RET_PACKET_STATUS(returnDefault.status);
}

void SensorCommander::setSkirtThreshNearLow(uint16_t value) {
  PacketSetSkirtThreshValue packet;
  packet.value = value;
  PacketReturnDefault returnDefault = sendCommandReceiveDefault(aeye::CMD_SET_SKIRT_THRESH_NEAR_LOW, &packet);
  CHECK_RET_PACKET_STATUS(returnDefault.status);
}

void SensorCommander::setSkirtThreshFarHigh(uint16_t value) {
  PacketSetSkirtThreshValue packet;
  packet.value = value;
  PacketReturnDefault returnDefault = sendCommandReceiveDefault(aeye::CMD_SET_SKIRT_THRESH_FAR_HIGH, &packet);
  CHECK_RET_PACKET_STATUS(returnDefault.status);
}

void SensorCommander::setSkirtThreshFarLow(uint16_t value) {
  PacketSetSkirtThreshValue packet;
  packet.value = value;
  PacketReturnDefault returnDefault = sendCommandReceiveDefault(aeye::CMD_SET_SKIRT_THRESH_FAR_LOW, &packet);
  CHECK_RET_PACKET_STATUS(returnDefault.status);
}

uint16_t SensorCommander::getSkirtThreshNearHigh() {
  sendCommand(aeye::CMD_GET_SKIRT_THRESH_NEAR_HIGH);
  PacketHeader returnHeader = readResponseHeader();
  PacketGetSkirtThreshValue body;
  CHECK_BODY_TYPE_AND_SIZE(returnHeader, aeye::CMD_GET_SKIRT_THRESH_NEAR_HIGH, body);
  ReceivePacketBody(&body, sizeof(body));
  CHECK_RET_PACKET_STATUS(body.status);
  return body.value;
}

uint16_t SensorCommander::getSkirtThreshNearLow() {
  sendCommand(aeye::CMD_GET_SKIRT_THRESH_NEAR_LOW);
  PacketHeader returnHeader = readResponseHeader();
  PacketGetSkirtThreshValue body;
  CHECK_BODY_TYPE_AND_SIZE(returnHeader, aeye::CMD_GET_SKIRT_THRESH_NEAR_LOW, body);
  ReceivePacketBody(&body, sizeof(body));
  CHECK_RET_PACKET_STATUS(body.status);
  return body.value;
}

uint16_t SensorCommander::getSkirtThreshFarHigh() {
  sendCommand(aeye::CMD_GET_SKIRT_THRESH_FAR_HIGH);
  PacketHeader returnHeader = readResponseHeader();
  PacketGetSkirtThreshValue body;
  CHECK_BODY_TYPE_AND_SIZE(returnHeader, aeye::CMD_GET_SKIRT_THRESH_FAR_HIGH, body);
  ReceivePacketBody(&body, sizeof(body));
  CHECK_RET_PACKET_STATUS(body.status);
  return body.value;
}

uint16_t SensorCommander::getSkirtThreshFarLow() {
  sendCommand(aeye::CMD_GET_SKIRT_THRESH_FAR_LOW);
  PacketHeader returnHeader = readResponseHeader();
  PacketGetSkirtThreshValue body;
  CHECK_BODY_TYPE_AND_SIZE(returnHeader, aeye::CMD_GET_SKIRT_THRESH_FAR_LOW, body);
  ReceivePacketBody(&body, sizeof(body));
  CHECK_RET_PACKET_STATUS(body.status);
  return body.value;
}

void SensorCommander::toggleHawkEye(bool enable, const std::string& clientIPAddress, uint16_t port) {
  if (clientIPAddress.length() > (sizeof(aeye::PacketToggleHawkEye::clientIp) - 1)) {
    throw std::runtime_error("Client IP address exceeds acceptable length.");
  }
  PacketToggleHawkEye body{};
  body.enable = enable ? 1 : 0;
  std::memcpy(body.clientIp, clientIPAddress.c_str(), sizeof(body.clientIp));
  body.hawkEyePort = port;
  try {
    aeye::PacketReturnDefault packetReturnDefault = sendCommandReceiveDefault(aeye::CMD_TOGGLE_HAWKEYE, &body);
    CHECK_RET_PACKET_STATUS(packetReturnDefault.status);
  } catch (std::exception &e) {
    spdlog::error("Command toggleHawkEye raised exception: `{}`.", e.what());
    throw;
  }
}

void SensorCommander::setHawkEyePoint(bool enable, uint16_t row, uint16_t col){
  aeye::PacketHawkEyePoint body;
  body.enable = enable ? 1 : 0;
  body.hawkEyePointCol = col;
  body.hawkEyePointRow = row;
  try {
    aeye::PacketReturnDefault packetReturnDefault = sendCommandReceiveDefault(aeye::CMD_SET_HAWKEYE_POINT_MODE, &body);
    CHECK_RET_PACKET_STATUS(packetReturnDefault.status);
  } catch (std::exception &e) {
    spdlog::error("Command setHawkEyePoint raised exception: `{}`.", e.what());
    throw;
  }
}

void SensorCommander::toggleARxSweepMode(bool enable, const std::string &clientIPAddress, uint16_t port,
                                         aeye::GainChannel gain_channel, aeye::PixelMode pixelMode, uint16_t colSize,
                                         uint16_t rowSize){
  aeye::PacketToggleARxSweep body{};
  body.enable = enable ? 1 : 0;
  body.gainChannel = gain_channel;
  body.pixelMode = pixelMode;
  body.colSize = colSize;
  body.rowSize = rowSize;
  std::memcpy(body.clientIp, clientIPAddress.c_str(), sizeof(body.clientIp));
  body.hawkEyePort = port;
  sendCommandReceiveDefault(aeye::CMD_SET_ADVANCED_RX_SWEEP, &body);
}

void SensorCommander::toggleARxStaticMode(bool enable, const std::string &clientIPAddress, uint16_t port, PixelMode pixelMode, uint32_t row, uint32_t col){
  aeye::PacketToggleARxStaticMode body;
  body.enable = enable ? 1 : 0;
  body.pixelMode = pixelMode;
  body.pixelRow = row;
  body.pixelCol = col;
  body.hawkEyePort = port;
  std::memcpy(body.clientIp, clientIPAddress.c_str(), sizeof(body.clientIp));
  sendCommandReceiveDefault(aeye::CMD_SET_ADVANCED_RX_STATIC_MODE, &body);
}


void SensorCommander::generateARxLUTV2(aeye::RxFOV rxFov, aeye::PixelMode pixelMode, uint8_t rxWindowSize) {
  aeye::PacketToggleARxLUT body{};
  body.rxFov = rxFov;
  body.pixelMode = pixelMode;
  body.rxWindowSize = rxWindowSize;
  sendCommandReceiveDefault(aeye::CMD_GENERATE_ARX_LUT_V2, &body);
}

void SensorCommander::getGenerateARxLutV2Status(bool &isRunning,
                                                uint8_t &calIterations,
                                                uint8_t &startScore,
                                                uint64_t &endScore,
                                                float &ratio) {

  sendCommand(aeye::CMD_GET_GENERATE_ARX_LUT_V2_STATUS);

  // Receive response
  PacketHeader returnHeader = readResponseHeader();
  PacketGetARxLUTV2Status packetGetARxLUTV2Status;

  CHECK_BODY_TYPE_AND_SIZE(returnHeader, aeye::CMD_GET_GENERATE_ARX_LUT_V2_STATUS, packetGetARxLUTV2Status);
  ReceivePacketBody(&packetGetARxLUTV2Status, sizeof(packetGetARxLUTV2Status));

  isRunning = packetGetARxLUTV2Status.isRunning;
  calIterations = packetGetARxLUTV2Status.calIterations;
  startScore = packetGetARxLUTV2Status.startScore;
  endScore = packetGetARxLUTV2Status.endScore;
  ratio = packetGetARxLUTV2Status.ratio;
}

void SensorCommander::sendNumMultiEchoes(uint8_t numEchoes) {
  aeye::PacketSetNumMultiEchoes body{numEchoes};
  sendCommandReceiveDefault(aeye::CMD_SET_NUM_MULTI_ECHOES, &body);
}

void SensorCommander::setModeMultiEchoes(MultiEchoMode mode) {
  aeye::PacketSetModeMultiEchoes body{mode};
  sendCommandReceiveDefault(aeye::CMD_SET_MODE_MULTI_ECHOES, &body);
}

PacketGetNumMultiEchoes SensorCommander::getNumMultiEchoes() {
  sendCommand(aeye::CMD_GET_NUM_MULTI_ECHOES);
  PacketHeader returnHeader = readResponseHeader();
  PacketGetNumMultiEchoes body;
  CHECK_BODY_TYPE_AND_SIZE(returnHeader, aeye::RET_GET_NUM_MULTI_ECHOES, body);
  ReceivePacketBody(&body, sizeof(body));
  CHECK_RET_PACKET_STATUS(body.status);
  return body;
}

PacketGetModeMultiEchoes SensorCommander::getModeMultiEchoes() {
  sendCommand(aeye::CMD_GET_MODE_MULTI_ECHOES);
  PacketHeader returnHeader = readResponseHeader();
  PacketGetModeMultiEchoes body;
  CHECK_BODY_TYPE_AND_SIZE(returnHeader, aeye::RET_GET_MODE_MULTI_ECHOES, body);
  ReceivePacketBody(&body, sizeof(body));
  CHECK_RET_PACKET_STATUS(body.status);
  return body;
}

void SensorCommander::getHawkeyeBuffer(uint32_t bufferCount) {
  aeye::PacketHawkeyeParams body{};
  body.bufferCount = bufferCount;
  sendCommandReceiveDefault(aeye::CMD_GET_HAWKEYE_BUFFER, &body);
}

void SensorCommander::setARxPixel(uint8_t col, uint8_t row, uint8_t pixelMode) {
  aeye::PacketHawkeyeParams body{};
  body.rxCol = col;
  body.rxRow = row;
  body.rxPixelMode = pixelMode;
  sendCommandReceiveDefault(aeye::CMD_SET_ARX_PIXEL, &body);
}

void SensorCommander::toggleScanningMode(bool enable) {
  PacketHawkeyeParams body{};
  body.enable = enable;
  auto ret = sendCommandReceiveDefault(aeye::CMD_HAWKEYE_SCANNING_MODE, &body);
  CHECK_RET_PACKET_STATUS(ret.status);
}

void SensorCommander::sendScanZoneParameters(std::vector<AdvancedZoneScanRegion>& scan_regions) {

  // Cap the number of zones to 5
  if (scan_regions.size() > MAX_NUMBER_OF_SCAN_ZONES) {
    throw std::runtime_error("Currently a maximum of 5 zones are supported for dynamic scanning.");
  }

  // Construct a packet
  aeye::PacketZoneScanParamsBody body{};
  body.zone_enable_mask = 0;
  uint16_t idx = 0;
  for (auto &scan_region: scan_regions) {
    body.zone_enable_mask += (1U << idx);
    body.zones[idx] = scan_region;
    idx++;
  }

  // Send the packet
  sendCommandReceiveDefault(aeye::CMD_SEND_ZONE_SCAN_PARAMS, &body);
}

void SensorCommander::killSensorService() {
  PacketReturnDefault packetReturnDefault = sendCommandReceiveDefault(aeye::CMD_KILL_SERVICE);
  CHECK_RET_PACKET_STATUS(packetReturnDefault.status);
}

PacketBuildInfoExtraBody SensorCommander::getBuildInfoExtra() {
  sendCommand(AEyeCommands::CMD_GET_BUILD_INFO_EXTRA);
  PacketHeader returnHeader = readResponseHeader();
  PacketBuildInfoExtraBody body{};
  CHECK_BODY_TYPE_AND_SIZE(returnHeader, aeye::CMD_GET_BUILD_INFO_EXTRA, body);
  ReceivePacketBody(&body, sizeof(body));
  return body;
}

void SensorCommander::setRxFOV(RxFOV rx_fov) {
  PacketSetRxFOVValue body{rx_fov};
  auto ret = sendCommandReceiveDefault(aeye::CMD_SET_RX_FOV, &body);
  CHECK_RET_PACKET_STATUS(ret.status);
}

void SensorCommander::setHFOV(float percentage) {
  PacketHFOVConfig body;
  body.hfov_percentage = percentage;
  auto ret = sendCommandReceiveDefault(aeye::CMD_SET_HFOV, &body);
  CHECK_RET_PACKET_STATUS(ret.status);
}

PacketGetHsyncParams SensorCommander::getHsyncParameters() {
  sendCommand(aeye::CMD_GET_HSYNC_PARAMETERS);
  PacketHeader returnHeader = readResponseHeader();
  PacketGetHsyncParams body;
  CHECK_BODY_TYPE_AND_SIZE(returnHeader, aeye::CMD_GET_HSYNC_PARAMETERS, body);
  ReceivePacketBody(&body, sizeof(body));
  return body;
}

void SensorCommander::SetTCPKeepAlive(bool enable) {
  PacketTCPKeepAlive body;
  body.enable = enable;
  auto ret = sendCommandReceiveDefault(aeye::CMD_SET_TCP_KEEP_ALIVE, &body);
  CHECK_RET_PACKET_STATUS(ret.status);
}

bool SensorCommander::GetTCPKeepAlive() {
  sendCommand(aeye::CMD_GET_TCP_KEEP_ALIVE);
  PacketHeader returnHeader = readResponseHeader();
  PacketTCPKeepAlive body;
  CHECK_BODY_TYPE_AND_SIZE(returnHeader, aeye::CMD_GET_HSYNC_PARAMETERS, body);
  ReceivePacketBody(&body, sizeof(body));
  return body.enable;
}
